#ifndef HUNGARIAN_ALGORITHM_HPP
#define HUNGARIAN_ALGORITHM_HPP
/**
 * HungarianAlgorithm
 * ------------------
 * 需求回顾（结合的反光柱场景）：
 * - 行 = 观测点 C_i（当前帧提取的反光柱），共 n 行
 * - 列 = 地标 L_j（地图里的反光柱），共 m 列
 * - 成本 = 距离（或马氏距离）c[i][j]；若超出门控阈值则视为“不可匹配”
 * - 允许“拒配 / 新地标”：给每一行一个**专属 dummy 列**，代价 = λ_i（行自适应的“拒配成本”）
 * - 运行“最小权完美匹配”（匈牙利算法），得到每一行恰好选 1 个列；
 *   若选到专属 dummy 列 ⇒ 该观测判定为“不匹配（-1）”
 *
 * 该实现要点：
 * 1) 门控：c > threshold → 设为 INF（极大数），表示不允许匹配。
 * 2) 加 n 个“专属 dummy 列”：第 i 行对自己的 dummy 列有代价 λ_i，其它行对该 dummy 列代价为 INF。
 *    这样就保证“每行都有至少一个可行选择”，并且互不冲突（每行一个 dummy）。
 * 3) 匈牙利算法本体要求“方阵”，我们把矩阵补成 S×S（S = max(n, m+n)），
 *    用 PAD 大数填充多余位置，保证不会影响真实匹配。
 *
 * 复杂度：O(S^3)，其中 S = max(n, m+n)；在工程里通常 n<<m+n，且门控后候选列更少，完全可用。
 */
class HungarianAlgorithm {
public:
  /**
   * Solve
   * @param cost_matrix  n×m，原始距离代价（米/马氏距离）；若无值请填 +inf
   * @param assignment   输出，长度 n；assignment[i]=j（0..m-1 表示匹配到地标 j；-1 表示拒配）
   * @param max_distance_threshold  门控阈值；>阈值的元素视为不可匹配
   *
   * 说明：
   * - 本函数会**自动**为每一行添加一个“专属 dummy 列”，并自动估计该行的拒配代价 λ_i。
   * - 若已知合适的 λ，可把 threshold 设为一个与系统一致的上界（例如 0.6m），
   *   函数内部会默认 λ_i = 0.5 * threshold（或略大于该行最小可行成本）。
   */
  void Solve(const std::vector<std::vector<double>>& cost_matrix,
             std::vector<int>& assignment,
             double max_distance_threshold = std::numeric_limits<double>::infinity())
  {
    const double INF = 1e12;      // “不可匹配”的极大数（远大于任何合理距离）
    const double PAD = 1e9;       // 补成方阵时的填充值（大），避免干扰真实最优
    const int n = (int)cost_matrix.size();
    const int m = n ? (int)cost_matrix[0].size() : 0;

    assignment.assign(n, -1);
    if (n == 0 || m == 0) return;

    // ---------- 第 1 步：门控（>threshold 置 INF） ----------
    std::vector<std::vector<double>> gated(n, std::vector<double>(m, INF));
    for (int i = 0; i < n; ++i) {
      for (int j = 0; j < m; ++j) {
        double c = cost_matrix[i][j];
        if (std::isfinite(c) && c <= max_distance_threshold) {
          gated[i][j] = c;
        } else {
          gated[i][j] = INF; // 不可行
        }
      }
    }

    // ---------- 第 2 步：为每行添加“专属 dummy 列” ----------
    // 列总数变为 m_aug = m + n
    const int m_aug = m + n;
    std::vector<std::vector<double>> aug(n, std::vector<double>(m_aug, INF));

    // 2.1 拷贝门控后的真实成本
    for (int i = 0; i < n; ++i) {
      for (int j = 0; j < m; ++j) {
        aug[i][j] = gated[i][j];
      }
    }

    // 2.2 估计每行的“拒配成本” λ_i，并将其放到专属 dummy 列 (m + i)
    //     直觉：λ_i 应该 “大于该行合理匹配的典型成本、但显著小于明显错误匹配”
    //     这里采取一个稳健、通用的默认：若 threshold 有意义，则 λ_i=0.5*threshold；
    //     否则 λ_i = row_min + margin（margin= max(5cm, 0.25*row_min)）
    for (int i = 0; i < n; ++i) {
      double row_min = INF;
      for (int j = 0; j < m; ++j) row_min = std::min(row_min, aug[i][j]);

      double lambda_i;
      if (std::isfinite(max_distance_threshold)) {
        lambda_i = 0.5 * max_distance_threshold;        // 常用：门限的一半
        if (std::isfinite(row_min)) {
          // 确保“拒配成本”略高于该行已存在的最小可行匹配成本，避免把“好配对”当成拒配
          double guard = row_min + std::max(0.05, 0.25 * row_min);
          lambda_i = std::max(lambda_i, guard);
        }
      } else {
        if (std::isfinite(row_min)) {
          lambda_i = row_min + std::max(0.05, 0.25 * row_min);
        } else {
          lambda_i = 1.0; // 没有任何可行匹配时的兜底拒配成本（单位米，可按需调大/小）
        }
      }
      // 设置专属 dummy 列
      for (int j = 0; j < n; ++j) {
        aug[i][m + j] = INF; // 其它行对该 dummy 列不可行
      }
      aug[i][m + i] = lambda_i; // 只有第 i 行可以选自己的 dummy
    }

    // ---------- 第 3 步：把 n×m_aug 补成 S×S 的方阵 ----------
    // 使用 PAD 大数填充多余位置，避免被算法“错误”选择。
    const int S = std::max(n, m_aug);
    std::vector<std::vector<double>> A(S, std::vector<double>(S, PAD));

    for (int i = 0; i < n; ++i) {
      for (int j = 0; j < m_aug; ++j) {
        A[i][j] = aug[i][j];
      }
    }
    // 其余 A[i][j] 维持 PAD。这样“完美匹配”一定存在（每行至少有自己的 dummy 可选），
    // 而多出来的行/列只以 PAD 形式存在，不会抢占真实的好位置。

    // ---------- 第 4 步：运行匈牙利算法（最小化） ----------
    // 经典实现（Kuhn-Munkres），变量含义如下：
    // - u[1..S], v[1..S]：行/列的势能（对偶变量）
    // - p[0..S]：列被谁占用的行（p[j]=i表示列 j 匹配到行 i）；p[0] 是虚拟列
    // - way[0..S]：用于恢复增广路径
    std::vector<double> u(S + 1), v(S + 1);
    std::vector<int> p(S + 1), way(S + 1);

    // 注意：匈牙利算法通常以“列”为主进行增广（这里沿用常见代码结构）
    for (int i = 1; i <= S; ++i) {
      p[0] = i;                     // 当前要增广的行 i
      int j0 = 0;                   // 从虚拟列 0 开始
      std::vector<double> minv(S + 1, INF);
      std::vector<char> used(S + 1, false);

      do {
        used[j0] = true;
        int i0 = p[j0], j1 = 0;
        double delta = INF;

        // 尝试所有“未使用的列 j”，寻找沿最短可行边的增广路径
        for (int j = 1; j <= S; ++j) {
          if (used[j]) continue;
          // 约化成本（reduced cost）：A[i0][j] - u[i0] - v[j]
          double cur = A[i0 - 1][j - 1] - u[i0] - v[j];
          if (cur < minv[j]) { minv[j] = cur; way[j] = j0; }
          if (minv[j] < delta) { delta = minv[j]; j1 = j; }
        }

        // 调整势能，使得至少一个未使用列的 minv 降为 0（制造新“零边”）
        for (int j = 0; j <= S; ++j) {
          if (used[j]) { u[p[j]] += delta; v[j] -= delta; }
          else         { minv[j] -= delta; }
        }
        j0 = j1; // 继续沿着产生“零边”的列前进
      } while (p[j0] != 0); // 一直增广到虚拟列（找到可增广路径）

      // 回溯增广路径：把路径上的匹配关系“翻转”
      do {
        int j1 = way[j0];
        p[j0] = p[j1];
        j0 = j1;
      } while (j0);
    }

    // ---------- 第 5 步：从 p[] 还原“列→行”的匹配，再转为“行→列” ----------
    // p[j]=i 表示：列 j 被行 i 匹配
    // 我们需要 assignment[i]（每行选到哪个原始列）
    std::vector<int> matchRowOfCol(S, -1);
    for (int j = 1; j <= S; ++j) {
      if (p[j] >= 1 && p[j] <= S) {
        matchRowOfCol[j - 1] = p[j] - 1; // 列 (j-1) ← 行 (p[j]-1)
      }
    }

    // 把每个真实行 i 的选择找出来：在所有列里找到 matchRowOfCol[col]==i 的 col
    for (int i = 0; i < n; ++i) {
      int chosen_col = -1;
      for (int col = 0; col < m_aug; ++col) {
        if (matchRowOfCol[col] == i) { chosen_col = col; break; }
      }
      if (chosen_col < 0) {
        // 理论上不会发生（每行至少能选自己的 dummy）
        assignment[i] = -1;
        continue;
      }
      // 若落在真实列 0..m-1：输出该地标索引；若落在专属 dummy 列 m+i：输出 -1
      if (chosen_col < m) assignment[i] = chosen_col;
      else                assignment[i] = -1; // 选到 dummy ⇒ 拒配/新地标
    }
  }
};
#endif
